#include "Communication/SubTwist.h"
#include "data_struct/base/DataSet.h"
#include "modules/MonitorModule/PostionStuckMonitor.h"


namespace behavior_controller {
namespace SubTwist {
static rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twistSub;

void sub_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {

    postion_stuck_monitor.SetPubSpeed(msg->linear.x, msg->angular.z);
}

void SubTwistInfo(rclcpp::Node::SharedPtr &node_handle) {
    twistSub = node_handle->create_subscription<geometry_msgs::msg::Twist>(
            "cmd_vel", rclcpp::SensorDataQoS(), sub_callback);
    std::cout << "SubTwist SubTwistInfo" << std::endl;
}
} // namespace SubTwist

} // namespace behavior_controller